#ifndef XYZTOLLA_XYZ_LLA_H
#define XYZTOLLA_XYZ_LLA_H

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <geometry_msgs/TwistWithCovarianceStamped.h> // gps/vel
#include <nav_msgs/Odometry.h>                        // gps/odom
#include <pcl/point_types.h>
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h> // gps/fix

#define r_earth 6.371393e6 // 地球半径
#define pi_ 3.14159

struct LogLatAlt {
  double log;
  double lat;
  double alt;
};

struct Angle {
  double sin;
  double cos;
};
class Xyz_trans {
public:
  Xyz_trans(const sensor_msgs::NavSatFix &gps_fix,
            const geometry_msgs::TwistWithCovarianceStamped &gps_vel,
            const nav_msgs::Odometry &gps_odom, const pcl::PointXYZ &xyz);
  Angle CalculateAngle();
  LogLatAlt CalculateLla();
  void CurrentPosShow();

private:
  pcl::PointXYZ origin_xyz_; // 探测到的点在gnss坐标系中的信息
  LogLatAlt log_lat_alt_;    // 车体当前经纬高信息
  Angle car_angle_; // 根据四个速度算出的车体前向相对于正北方向的角度信息
  Eigen::Quaterniond qua_heading_; // 车子相对地球姿态四元数
};

#endif // XYZTOLLA_XYZ_LLA_H
